﻿using System;
using System.Collections.Generic;
using System.Text;
using RoverStudio.Common.PositionSource;
using PositionSource.WebCam.Interfaces;
using CoordinateCalculator;
using RoverStudio.Math;
using Clustering;
using System.IO;
using System.Drawing.Imaging;

namespace WebcamPositionSourceConfiguration.Clustering
{
    public class WebcamLEDClusteringPositionSourceConfiguration
    {
        private IVideoSourceCommon videoSource;

        public IVideoSourceCommon VideoSource
        {
            get { return videoSource; }
        }

        public event PositionUpdatedHandler Rover1PositionUpdated;
        
        private DelegateWrapperPositionSource dwps1;
        public IPositionSource Rover1Position
        {
            get
            {
                return dwps1;
            }
        }

        
        public event PositionUpdatedHandler Rover2PositionUpdated;
        
        private DelegateWrapperPositionSource dwps2;
        public IPositionSource Rover2Position
        {
            get
            {
                return dwps2;
            }
        }

        private ILedPositionExtractor ledPositionExtractor1;
        private ILedPositionExtractor ledPositionExtractor2;
        private World3DFromPicture2D transformer;

        public World3DFromPicture2D Transformer
        {
            get { return transformer; }
        }

        private KMeansClusteringVideoSeparator clusteringVideoSeparator;

        public KMeansClusteringVideoSeparator ClusteringVideoSeparator
        {
            get { return clusteringVideoSeparator; }
            //set { clusteringVideoSeparator = value; }
        }

        private volatile bool setreference = false;
        private volatile int saveDebugPictures = 0;

        Vector3D refPoint = new Vector3D(0, 0, 0);
        Rotation refRotationInv = new Rotation();

        public WebcamLEDClusteringPositionSourceConfiguration(IVideoSourceCommon videoSource)
        {
            this.videoSource = videoSource;
            ledPositionExtractor1 = new LedPositionExtractor();
            ledPositionExtractor2 = new LedPositionExtractor();
            transformer = new World3DFromPicture2D();
            dwps1 = new DelegateWrapperPositionSource("Rover1 Position", ref Rover1PositionUpdated);
            dwps2 = new DelegateWrapperPositionSource("Rover2 Position", ref Rover2PositionUpdated);
            clusteringVideoSeparator = new KMeansClusteringVideoSeparator(videoSource);
            clusteringVideoSeparator.VideoOutput1.NewFrame += new NewFrameDelegate(VideoOutput1_NewFrame);
            clusteringVideoSeparator.VideoOutput2.NewFrame += new NewFrameDelegate(VideoOutput2_NewFrame);
        }

        private string debugDirectory = System.Configuration.ConfigurationManager.AppSettings["DebugDirectory"];
        private int errorImageNumber = 0;
        private int GetNextErrorImageNumber()
        {
            return errorImageNumber++;
        }
        void VideoOutput1_NewFrame(System.Drawing.Bitmap frame)
        {
            frame = ProcessNewFrame(frame, 1, ledPositionExtractor1, Rover1PositionUpdated);
        }

        void VideoOutput2_NewFrame(System.Drawing.Bitmap frame)
        {
            frame = ProcessNewFrame(frame, 2, ledPositionExtractor2, Rover2PositionUpdated);
        }

        private System.Drawing.Bitmap ProcessNewFrame(System.Drawing.Bitmap frame, int roverNum, ILedPositionExtractor ledPositionExtractor, PositionUpdatedHandler roverPositionUpdated)
        {
            string debugFileName = null;
            bool savedebug = saveDebugPictures>0;
            try
            {

                if (savedebug)
                {
                    debugFileName = "r" + roverNum + "_" + DateTime.Now.Ticks;
                    Directory.CreateDirectory(@"d:\debug\picout");
                    frame.Save(@"d:\debug\picout\" + debugFileName + "_0original.png", ImageFormat.Png);
                }
                DateTime now = DateTime.Now;
                Vector2D[] leds = ledPositionExtractor.GetLEDPositionsFromImage(ref frame);
                if (leds == null)
                {
                    leds = ledPositionExtractor.GetLEDPositionsFromImage(ref frame);
                }
                if (leds == null)
                {
                    leds = ledPositionExtractor.GetLEDPositionsFromImage(ref frame);
                }
                if (savedebug)
                    frame.Save(@"d:\debug\picout\" + debugFileName + "_1afterled.png", ImageFormat.Png);
                Vector3D position;
                Rotation orientation;
                transformer.CalculateTransformation(leds, out position, out orientation);

                if (setreference)
                {
                    setreference = false;
                    refPoint = new Vector3D(position);
                    refRotationInv = orientation.Inverse();
                    //ledPositionExtractor1.FramesToAverage = fta;
                }

                position = refRotationInv.Apply(position - refPoint);
                orientation = refRotationInv * orientation;


                if (roverPositionUpdated != null)
                    roverPositionUpdated(position, orientation, now);

            }
            catch (Exception e)
            {
                if (savedebug)
                {
                    frame.Save(@"d:\debug\picout\" + debugFileName + "_2exception.png", ImageFormat.Png);
                    string error = "exception: "+e.ToString()+"\r\n"+e.StackTrace+"\r\n";
                    File.WriteAllText(@"d:\debug\picout\" + debugFileName + "_3exception.txt", error);
                }
                //string dir = Path.Combine(debugDirectory, "led");
                //int img_num = GetNextErrorImageNumber();
                //if (!Directory.Exists(dir))
                //    Directory.CreateDirectory(dir);
                //frame.Save(Path.Combine(dir, "img_rover" + roverNum + "_" + img_num + ".png"), ImageFormat.Png);
            }
            finally
            {
                saveDebugPictures--;
            }
            return frame;
        }



        public class DelegateWrapperPositionSource : IPositionSource
        {
            //private WebcamLEDClusteringPositionSourceConfiguration parent;

            public DelegateWrapperPositionSource(string name, ref PositionUpdatedHandler sourceDelegate)
            {
                sourceDelegate += new PositionUpdatedHandler(OnPositionUpdated);
            }

            private void OnPositionUpdated(Vector3D pos, Rotation rot, DateTime time)
            {
                if (PositionUpdated != null)
                    PositionUpdated(pos, rot, time);
            }

            #region IPositionSource Members

            public event PositionUpdatedHandler PositionUpdated;

            public void Start()
            {
                throw new NotImplementedException();
            }

            public void Stop()
            {
                throw new NotImplementedException();
            }

            #endregion
        }

        public void SetReferenceTransformation()
        {
            setreference = true;
        }

        public void SaveDebugPictures()
        {
            saveDebugPictures = 2;
        }
    }
}
